//----------------------------------------------------------------------------
// M A R I T I M E  R E S E A R C H  I N S T I T U T E  N E T H E R L A N D S
//----------------------------------------------------------------------------
// Copyright (C) 2008 - MARIN - All rights reserved - http://www.marin.nl
//----------------------------------------------------------------------------
// Program    : mermaid
// Module     : vbm
// File       : RrbSolver.cpp
//----------------------------------------------------------------------------

#include <math.h>
#include "RrbSolver.h"
#include "FloatUtils.h"
#include "Timer.h"

#include "omp.h"

#include "assertplus.h"

#ifdef _WINDOWS
#pragma warning(disable: 4244) // conversion from 'double' to 'float', possible loss of data
#endif

#define COLUMNS  (unsigned)(Nx1 + 3)
#define ROWS     (unsigned)(Nx2 + 3)
#define COLUMNS2 (2 * (Nx1 + 2) + 1)
#define ROWS2    (2 * (Nx2 + 2) + 1)

// these may not be member variables because they are used in openmp reductions
static REAL rhoold = 0.0;
static REAL rhonew = 0.0;
static REAL pq     = 0.0;

/// constructor
RrbSolver::RrbSolver(Array2D<REAL> *pcc,
                     Array2D<REAL> *pcs,
                     Array2D<REAL> *pcw,
                     int Nx1,
                     int Nx2,
                     REAL tolerance)
: base(Nx1, Nx2, tolerance)
, m_pcc(pcc)
, m_pcs(pcs)
, m_pcw(pcw)
, m_cne(COLUMNS, ROWS)
, m_cnw(COLUMNS, ROWS)
, m_cse(COLUMNS2, ROWS2)
, m_csw(COLUMNS, ROWS)
, m_ccc(COLUMNS, ROWS)
, m_css(COLUMNS, ROWS)
, m_cww(COLUMNS, ROWS)
, m_cee(COLUMNS, ROWS)
, m_cnn(COLUMNS, ROWS)
, m_st(0)
, m_tw(0)
, m_niv(0)
, m_rhobew(0.0)
, m_bet(0.0)
, m_alf(0.0)
, m_done(false)
, m_done1(false)
, m_done2(false)
{
    ASSERT(pcc->dim1() == COLUMNS && pcc->dim2() == ROWS, "c array size mismatch");
    ASSERT(pcs->dim1() == COLUMNS && pcs->dim2() == ROWS, "s array size mismatch");
    ASSERT(pcw->dim1() == COLUMNS && pcw->dim2() == ROWS, "w array size mismatch");
    preconditioner();
}

/// destructor
RrbSolver::~RrbSolver()
{
//#ifdef spy
//    PrintArray2Dcontents("", m_cne, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
//    PrintArray2Dcontents("", m_cnw, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
//    PrintArray2Dcontents("", m_cse, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
//    PrintArray2Dcontents("", m_csw, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
//    PrintArray2Dcontents("", m_ccc, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
//    PrintArray2Dcontents("", m_css, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
//    PrintArray2Dcontents("", m_cww, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
//    PrintArray2Dcontents("", m_cee, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
//    PrintArray2Dcontents("", m_cnn, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
//
//    PlotArrayContents("", m_cne, 1, m_Nx1, 1, m_Nx2);
//    PlotArrayContents("", m_cnw, 1, m_Nx1, 1, m_Nx2);
//    PlotArrayContents("", m_cse, 1, m_Nx1, 1, m_Nx2);
//    PlotArrayContents("", m_csw, 1, m_Nx1, 1, m_Nx2);
//    PlotArrayContents("", m_ccc, 1, m_Nx1, 1, m_Nx2);
//    PlotArrayContents("", m_css, 1, m_Nx1, 1, m_Nx2);
//    PlotArrayContents("", m_cww, 1, m_Nx1, 1, m_Nx2);
//    PlotArrayContents("", m_cee, 1, m_Nx1, 1, m_Nx2);
//    PlotArrayContents("", m_cnn, 1, m_Nx1, 1, m_Nx2);
//#endif
}

/// preconditioner
/** Deze subroutine maakt een LDL'-ontbinding van een symmetrische, pos. sem. 
    definiete matrix volgens de RRB-methode. \n
    L' en D worden opgeslagen in stencil-vorm. 
    L' wordt in opgeslagen in de arrays cnn, css, cee en cww. \n
    Omdat het nieuwe Schurcomplement telkens gerepresenteerd kan worden als een 
    negen-punts stencil, hebben we tijdens de incomplete ontbinding als kladarrays 
    nodig: cse, csw, cne en cnw. \n
    Het laatste stuk wordt exact gedaan. Het laatste Schur-complement heeft 
    grootte sg en bandbreedte bsm. Een incomplete ontbinding van het laatste 
    Schur-complement wordt opgeslagen in s.  */
void RrbSolver::preconditioner()
{
    Array2D<REAL> &cc = *m_pcc;
    Array2D<REAL> &cs = *m_pcs;
    Array2D<REAL> &cw = *m_pcw;

    // Als eerste stap de matrix A copieren naar ccc, cww, enz. 
    m_ccc = cc;
    m_css = cs;
    m_cww = cw;
    int ii = 0;
    int jj = 0;
    for (jj = 0; jj <= m_Nx2 + 1; ++jj)
    {
        for (ii = 0; ii <= m_Nx1 + 1; ++ii)
        {
            m_cnn[ii][jj] = cs[ii ][jj + 1];
            m_cee[ii][jj] = cw[ii + 1][jj ];
        }
    }
    m_cne = 0.0;
    m_cnw = 0.0;
    m_cse = 0.0;
    m_csw = 0.0;

    int st = 1;
    int tw = 2;
    int niv = 0;
    while (tw < m_Nx2 + 2)
    {
        //                                 cnw cnn cne 
        // Ga uit van een 9-punts stencil  cww ccc cee 
        //                                 csw css cse 
        // Doorloop eerst een schaakbord met daarin cnw, cne, ccc, csw en cse 
        // als zwarte punten in S11. Lump cnw, cne, csw en cse op de rest. 
        // Dit geeft een 5-punts stencil en diagonaalmatrix S11. 
        //                                     cnn 
        //                                 cww ccc cee 
        //                                     css 
        if (niv > 0)
        {
            REAL fac = 1.0 + st * 10.0 / ((m_Nx1 + 2) * (m_Nx2 + 2));
            for (jj = st; jj <= m_Nx2 + 2; jj += tw)
            {
                for (ii = 0; ii <= m_Nx1 + 2; ii += tw)
                {
                    m_ccc[ii][jj] = m_ccc[ii][jj] * fac +
                        m_cne[ii][jj] + m_cnw[ii][jj] +
                        m_cse[ii][jj] + m_csw[ii][jj];
                }
            }
            for (jj = 0; jj <= m_Nx2 + 2; jj += tw)
            {
                for (ii = st; ii <= m_Nx1 + 2; ii += tw)
                {
                    m_ccc[ii][jj] = m_ccc[ii][jj] * fac +
                        m_cne[ii][jj] + m_cnw[ii][jj] +
                        m_cse[ii][jj] + m_csw[ii][jj];
                }
            }
        }
        ++niv;

        // Voor de overblijvende witte punten hebben we eveneens 
        //                      cnw cnn cne 
        // een 9-punts stencil  cww ccc cee 
        //                      csw css cse 
        // met dus cnw, cne, ccc, csw en cse als witte punten. 
        // Op de witte punten denken we een nieuw schaakbord aangebracht => 
        // S22 en S33. We elimineren nu alle zwarte punten met de zojuist 
        // afgeleide 5-punts formule voor de zwarte punten. We krijgen dan 
        //                          cnn 
        //                      cnw     cne 
        //                  cww     ccc     cee 
        //                      csw     cse 
        //                          css 
        for (jj = st; jj <= m_Nx2 + 2; jj += tw)
        {
            for (ii = st; ii <= m_Nx1 + 2; ii += tw)
            {
                int iw = ii - st;
                int ie = ii + st;
                int js = jj - st;
                int jn = jj + st;
                if (jn <= m_Nx2 + 2)
                {
                    m_cne[ii][jj] = m_cne[ii][jj] - m_cnn[ii][jj] * m_cee[ii][jn] / m_ccc[ii][jn];
                    m_cnw[ii][jj] = m_cnw[ii][jj] - m_cnn[ii][jj] * m_cww[ii][jn] / m_ccc[ii][jn];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cnn[ii][jj] * m_css[ii][jn] / m_ccc[ii][jn];
                    m_cnn[ii][jj] = -m_cnn[ii][jj] * m_cnn[ii][jn] / m_ccc[ii][jn];
                }
                if (js > 0)
                {
                    m_cse[ii][jj] = m_cse[ii][jj] - m_css[ii][jj] * m_cee[ii][js] / m_ccc[ii][js];
                    m_csw[ii][jj] = m_csw[ii][jj] - m_css[ii][jj] * m_cww[ii][js] / m_ccc[ii][js];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_css[ii][jj] * m_cnn[ii][js] / m_ccc[ii][js];
                    m_css[ii][jj] = -m_css[ii][jj] * m_css[ii][js] / m_ccc[ii][js];
                }
                if (ie <= m_Nx1 + 2)
                {
                    m_cne[ii][jj] = m_cne[ii][jj] - m_cee[ii][jj] * m_cnn[ie][jj] / m_ccc[ie][jj];
                    m_cse[ii][jj] = m_cse[ii][jj] - m_cee[ii][jj] * m_css[ie][jj] / m_ccc[ie][jj];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cee[ii][jj] * m_cww[ie][jj] / m_ccc[ie][jj];
                    m_cee[ii][jj] = -m_cee[ii][jj] * m_cee[ie][jj] / m_ccc[ie][jj];
                }
                if (iw > 0)
                {
                    m_cnw[ii][jj] = m_cnw[ii][jj] - m_cww[ii][jj] * m_cnn[iw][jj] / m_ccc[iw][jj];
                    m_csw[ii][jj] = m_csw[ii][jj] - m_cww[ii][jj] * m_css[iw][jj] / m_ccc[iw][jj];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cww[ii][jj] * m_cee[iw][jj] / m_ccc[iw][jj];
                    m_cww[ii][jj] = -m_cww[ii][jj] * m_cww[iw][jj] / m_ccc[iw][jj];
                }
            }
        }
        for (jj = 0; jj <= m_Nx2 + 2; jj += tw)
        {
            for (ii = 0; ii <= m_Nx1 + 2; ii += tw)
            {
                int iw = ii - st;
                int ie = ii + st;
                int js = jj - st;
                int jn = jj + st;
                if (jn <= m_Nx2 + 2)
                {
                    m_cne[ii][jj] = m_cne[ii][jj] - m_cnn[ii][jj] * m_cee[ii][jn] / m_ccc[ii][jn];
                    m_cnw[ii][jj] = m_cnw[ii][jj] - m_cnn[ii][jj] * m_cww[ii][jn] / m_ccc[ii][jn];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cnn[ii][jj] * m_css[ii][jn] / m_ccc[ii][jn];
                    m_cnn[ii][jj] = -m_cnn[ii][jj] * m_cnn[ii][jn] / m_ccc[ii][jn];
                }
                if (js > 0)
                {
                    m_cse[ii][jj] = m_cse[ii][jj] - m_css[ii][jj] * m_cee[ii][js] / m_ccc[ii][js];
                    m_csw[ii][jj] = m_csw[ii][jj] - m_css[ii][jj] * m_cww[ii][js] / m_ccc[ii][js];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_css[ii][jj] * m_cnn[ii][js] / m_ccc[ii][js];
                    m_css[ii][jj] = -m_css[ii][jj] * m_css[ii][js] / m_ccc[ii][js];
                }
                if (ie <= m_Nx1 + 2)
                {
                    m_cne[ii][jj] = m_cne[ii][jj] - m_cee[ii][jj] * m_cnn[ie][jj] / m_ccc[ie][jj];
                    m_cse[ii][jj] = m_cse[ii][jj] - m_cee[ii][jj] * m_css[ie][jj] / m_ccc[ie][jj];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cee[ii][jj] * m_cww[ie][jj] / m_ccc[ie][jj];
                    m_cee[ii][jj] = -m_cee[ii][jj] * m_cee[ie][jj] / m_ccc[ie][jj];
                }
                if (iw > 0)
                {
                    m_cnw[ii][jj] = m_cnw[ii][jj] - m_cww[ii][jj] * m_cnn[iw][jj] / m_ccc[iw][jj];
                    m_csw[ii][jj] = m_csw[ii][jj] - m_cww[ii][jj] * m_css[iw][jj] / m_ccc[iw][jj];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cww[ii][jj] * m_cee[iw][jj] / m_ccc[iw][jj];
                    m_cww[ii][jj] = -m_cww[ii][jj] * m_cww[iw][jj] / m_ccc[iw][jj];
                }
            }
        }

        // Voor de overblijvende witte punten hebben we 
        //                          cnn 
        //                      cnw     cne 
        //                   cww    ccc    cee 
        //                      csw     cse 
        //                          css 
        // Hierin lumpen we de locale coefficienten cnloc etc. op de rest 
        //  => scheve 5-punts formule 
        //                      cnw     cne 
        //                          ccc 
        //                      csw     cse 
        for (jj = st; jj <= m_Nx2 + 2; jj += tw)
        {
            for (ii = st; ii <= m_Nx1 + 2; ii += tw)
            {
                m_ccc[ii][jj] = m_ccc[ii][jj] +
                    m_cnn[ii][jj] +
                    m_cee[ii][jj] +
                    m_css[ii][jj] +
                    m_cww[ii][jj];

                // Om geheugen te besparen gaan we cnw(ii,jj) opslaan in cnn(ii,jj);
                //                                 cne(ii,jj) opslaan in cee(ii,jj);
                //                                 csw(ii,jj) opslaan in cww(ii,jj);
                //                                 cse(ii,jj) opslaan in css(ii,jj);
                m_cnn[ii][jj] = m_cnw[ii][jj];
                m_cee[ii][jj] = m_cne[ii][jj];
                m_cww[ii][jj] = m_csw[ii][jj];
                m_css[ii][jj] = m_cse[ii][jj];
            }
        }
        ++niv;

        // Uitgaande van het stencil 
        //                          cnn 
        //                     cnw      cne 
        //                 cww      ccc      cee 
        //                     csw      cse 
        //                          css 
        // elimineren we cnw en cne met de scheve 5-punts formule. Dit geeft 
        //                 cnw      cnn      cne 
        //                 cww      ccc      cee 
        //                 csw      css      cse 
        // We zijn nu weer terug de eerste 9-punts formule, maar op een 
        // grover rooster. 
        for (jj = 0; jj <= m_Nx2 + 2; jj += tw)
        {
            for (ii = 0; ii <= m_Nx1 + 2; ii += tw)
            {
                int iw = ii - st;
                int ie = ii + st;
                int js = jj - st;
                int jn = jj + st;
                if (jn <= m_Nx2 + 2 && ie <= m_Nx1 + 2)
                {
                    m_cnn[ii][jj] = m_cnn[ii][jj] - m_cne[ii][jj] * m_cnw[ie][jn] / m_ccc[ie][jn];
                    m_cee[ii][jj] = m_cee[ii][jj] - m_cne[ii][jj] * m_cse[ie][jn] / m_ccc[ie][jn];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cne[ii][jj] * m_csw[ie][jn] / m_ccc[ie][jn];
                    m_cne[ii][jj] = -m_cne[ii][jj] * m_cne[ie][jn] / m_ccc[ie][jn];
                }
                if (iw > 0 && jn <= m_Nx2 + 2)
                {
                    m_cnn[ii][jj] = m_cnn[ii][jj] - m_cnw[ii][jj] * m_cne[iw][jn] / m_ccc[iw][jn];
                    m_cww[ii][jj] = m_cww[ii][jj] - m_cnw[ii][jj] * m_csw[iw][jn] / m_ccc[iw][jn];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cnw[ii][jj] * m_cse[iw][jn] / m_ccc[iw][jn];
                    m_cnw[ii][jj] = -m_cnw[ii][jj] * m_cnw[iw][jn] / m_ccc[iw][jn];
                }
                if (js > 0 && iw > 0)
                {
                    m_css[ii][jj] = m_css[ii][jj] - m_csw[ii][jj] * m_cse[iw][js] / m_ccc[iw][js];
                    m_cww[ii][jj] = m_cww[ii][jj] - m_csw[ii][jj] * m_cnw[iw][js] / m_ccc[iw][js];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_csw[ii][jj] * m_cne[iw][js] / m_ccc[iw][js];
                    m_csw[ii][jj] = -m_csw[ii][jj] * m_csw[iw][js] / m_ccc[iw][js];
                }
                if (js > 0 && ie <= m_Nx1 + 2)
                {
                    m_css[ii][jj] = m_css[ii][jj] - m_cse[ii][jj] * m_csw[ie][js] / m_ccc[ie][js];
                    m_cee[ii][jj] = m_cee[ii][jj] - m_cse[ii][jj] * m_cne[ie][js] / m_ccc[ie][js];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cse[ii][jj] * m_cnw[ie][js] / m_ccc[ie][js];
                    m_cse[ii][jj] = -m_cse[ii][jj] * m_cse[ie][js] / m_ccc[ie][js];
                }
            }
        }
        st = tw;
        tw *= 2;
    }

    // Achteraf delen door hoofddiagonaalelement om 
    // incomplete LDL'-ontbinding te krijgen: 
    for (ii = 0; ii <= m_Nx1 + 2; ++ii)
    {
        for (jj = 0; jj <= m_Nx2 + 2; ++jj)
        {
            m_ccc[ii][jj] = 1.0 / m_ccc[ii][jj];
            m_cnn[ii][jj] = m_cnn[ii][jj] * m_ccc[ii][jj];
            m_css[ii][jj] = m_css[ii][jj] * m_ccc[ii][jj];
            m_cee[ii][jj] = m_cee[ii][jj] * m_ccc[ii][jj];
            m_cww[ii][jj] = m_cww[ii][jj] * m_ccc[ii][jj];
        }
    }

    // Om deling door nul te voorkomen wordt voor alle witte punten 
    // van een schaakbordvolgorde niet de hoofddiagonaal van 
    // de matrix opgeslagen, maar 1/(--). 
    for (jj = 0; jj <= m_Nx2 + 2; jj += 2)
    {
        for (ii = 1; ii <= m_Nx1 + 2; ii += 2)
        {
            cc[ii][jj] = 1.0 / cc[ii][jj];
        }
    }
    for (jj = 1; jj <= m_Nx2 + 2; jj += 2)
    {
        for (ii = 0; ii <= m_Nx1 + 2; ii += 2)
        {
            cc[ii][jj] = 1.0 / cc[ii][jj];
        }
    }
    // De complete Choleski-ontbinding van het laatste stuk is nu gemaakt. 
} // preconditioner

/// solves system
void RrbSolver::solver()
{
    // N.B. in de oorspronkelijke code had m_cse de naam Az
    // Deze member variabele is de enige die in deze routine wordt gewijzigd
    // Eerst oplossen L(Az)=Az. We maken hierbij gebruik van symmetrie.
    // Note in the original code had m_cse called Az
         // This member variable is the only one in this routine is changed
         // First solve L (Az) = Az. We do this by using symmetry.

    const int nx1 = m_Nx1 + 1;
    const int nx2 = m_Nx2 + 1;
    const int nx12 = m_Nx1 + 2;
    const int nx22 = m_Nx2 + 2;

    { // -----------------------------------------------------------------------
        m_st = 1;
        m_tw = 2;
        m_niv = 0;
        ASSERT(nx22 >= m_tw, "m_tw < m_Nx2 + 2 : " << m_tw << "is not smaller than" << nx22);
    } // master ----------------------------------------------------------------

    // note: the following is the same piece of code as the piece at the end of the next while loop
    // Phase 1: 19.7.2 Do the forward substitution for the points (ii,jj): Updating r2-nodes using r1-nodes in the Same level

    for (int ii = m_st; ii <= nx1; ii += m_tw)
    {
        for (int jj = m_st; jj <= nx2; jj += m_tw)
        {
            REAL cse = m_cse[ii][jj];
            m_cse[ii + m_st][jj - m_st] -= m_css[ii][jj] * cse;
            m_cse[ii - m_st][jj - m_st] -= m_cww[ii][jj] * cse;
            m_cse[ii + m_st][jj + m_st] -= m_cee[ii][jj] * cse;
            m_cse[ii - m_st][jj + m_st] -= m_cnn[ii][jj] * cse;
        }
    }
    // for ---------------------------------------------------------------------

    { // -----------------------------------------------------------------------
        m_niv += 2;
        m_st = m_tw;
        m_tw *= 2;
    } // master ----------------------------------------------------------------

    // end of repeated code

    { // -----------------------------------------------------------------------
        m_done1 = m_tw >= nx22;
    } // master ----------------------------------------------------------------

loop1: // while m_tw < nx22

    if (m_done1)
    {
        goto endloop1;
    }

    { // -----------------------------------------------------------------------
        if (m_st <= nx1)
        {
            for (int jj = m_st; jj <= nx2; jj += m_tw)
            {
                REAL cse = m_cse[0][jj];
                m_cse[0 ][jj - m_st] -= m_css[0][jj] * cse;
                m_cse[m_st][jj     ] -= m_cee[0][jj] * cse;
                m_cse[0 ][jj + m_st] -= m_cnn[0][jj] * cse;
            }
        }

        if (m_st <= nx2)
        {
            for (int ii = m_st; ii <= nx1; ii += m_tw)
            {
                REAL cse = m_cse[ii][0];
                m_cse[ii + m_st][0 ] -= m_cee[ii][0] * cse;
                m_cse[ii     ][m_st] -= m_cnn[ii][0] * cse;
                m_cse[ii - m_st][0 ] -= m_cww[ii][0] * cse;
            }
        }
    } // master ---------------------------------------------------------------

    for (int ii = m_tw; ii <= nx1; ii += m_tw)
    {
        for (int jj = m_st; jj <= nx2; jj += m_tw)
        {
            REAL cse = m_cse[ii][jj];
            m_cse[ii     ][jj - m_st] -= m_css[ii][jj] * cse;
            m_cse[ii + m_st][jj     ] -= m_cee[ii][jj] * cse;
            m_cse[ii     ][jj + m_st] -= m_cnn[ii][jj] * cse;
            m_cse[ii - m_st][jj     ] -= m_cww[ii][jj] * cse;
        }
    }
    // for ---------------------------------------------------------------------

    for (int ii = m_st; ii <= nx1; ii += m_tw)
    {
        for (int jj = m_tw; jj <= nx2; jj += m_tw)
        {
            REAL cse = m_cse[ii][jj];
            m_cse[ii     ][jj - m_st] -= m_css[ii][jj] * cse;
            m_cse[ii + m_st][jj     ] -= m_cee[ii][jj] * cse;
            m_cse[ii     ][jj + m_st] -= m_cnn[ii][jj] * cse;
            m_cse[ii - m_st][jj     ] -= m_cww[ii][jj] * cse;
        }
    }
    // for ---------------------------------------------------------------------

    // note: the following is the same piece of code as the piece preceding this while loop
    for (int ii = m_st; ii <= nx1; ii += m_tw)
    {
        for (int jj = m_st; jj <= nx2; jj += m_tw)
        {
            REAL cse = m_cse[ii][jj];
            m_cse[ii + m_st][jj - m_st] -= m_css[ii][jj] * cse;
            m_cse[ii - m_st][jj - m_st] -= m_cww[ii][jj] * cse;
            m_cse[ii + m_st][jj + m_st] -= m_cee[ii][jj] * cse;
            m_cse[ii - m_st][jj + m_st] -= m_cnn[ii][jj] * cse;
        }
    }
    // for ---------------------------------------------------------------------

    { // -----------------------------------------------------------------------
        m_niv += 2;
        m_st = m_tw;
        m_tw *= 2;
    } // master ----------------------------------------------------------------

    // end of repeated code

    { // -----------------------------------------------------------------------
        if (!m_done1 && m_tw >= nx22)
        {
            m_done1 = true;
        }
    } // critical --------------------------------------------------------------

    if (!m_done1)
    {
        goto loop1;
    }

endloop1:; // while m_tw < nx22

    // Nu oplossen D(Az)=Az:
    for (int ii = 1; ii <= nx12; ii += 2)
    {
        for (int jj = 1; jj <= nx22; jj += 2)
        {
            m_cse[ii    ][jj    ] *= m_ccc[ii    ][jj    ];
            m_cse[ii - 1][jj - 1] *= m_ccc[ii - 1][jj - 1];
        }
    }
    // for ---------------------------------------------------------------------

    // Nu oplossen U(Az)=Az:
    { //------------------------------------------------------------------------
        m_done2 = m_st <= 2;
// temporary?    } // master ---------------------------------------------------------------

loop2: // while (m_st > 2)
    if (m_done2)
    {
        goto endloop2;
    }

    // Nu oplossen U(Az)=Az:
    // note: the following is the same piece of code as the piece following this while loop

    { //------------------------------------------------------------------------
        m_tw = m_st;
        m_st /= 2;
        m_niv -= 2;
    } // master ---------------------------------------------------------------

    for (int ii = m_st; ii <= nx1; ii += m_tw)
    {
        for (int jj = m_st; jj <= nx2; jj += m_tw)
        {
            m_cse[ii][jj] -= m_css[ii][jj] * m_cse[ii + m_st][jj - m_st] +
                             m_cww[ii][jj] * m_cse[ii - m_st][jj - m_st] +
                             m_cee[ii][jj] * m_cse[ii + m_st][jj + m_st] +
                             m_cnn[ii][jj] * m_cse[ii - m_st][jj + m_st];
        }
    }
    // for ---------------------------------------------------------------------

    // end of repeated code

    { //------------------------------------------------------------------------
        if (m_st <= nx1)
        {
            for (int jj = m_st; jj <= nx2; jj += m_tw)
            {
                m_cse[0][jj] -= m_css[0][jj] * m_cse[0 ][jj - m_st] +
                                m_cee[0][jj] * m_cse[m_st][jj     ] +
                                m_cnn[0][jj] * m_cse[0 ][jj + m_st];
            }
        }

        if (m_st <= nx2)
        {
            for (int ii = m_st; ii <= nx1; ii += m_tw)
            {
                m_cse[ii][0] -= m_cww[ii][0] * m_cse[ii - m_st][0 ] +
                                m_cee[ii][0] * m_cse[ii + m_st][0 ] +
                                m_cnn[ii][0] * m_cse[ii     ][m_st];
            }
        }
    } // master ----------------------------------------------------------------

    for (int ii = m_tw; ii <= nx1; ii += m_tw)
    {
        for (int jj = m_st; jj <= nx2; jj += m_tw)
        {
            m_cse[ii][jj] -=  m_css[ii][jj] * m_cse[ii     ][jj - m_st] +
                              m_cww[ii][jj] * m_cse[ii - m_st][jj     ] +
                              m_cee[ii][jj] * m_cse[ii + m_st][jj     ] +
                              m_cnn[ii][jj] * m_cse[ii     ][jj + m_st];
        }
    }
    // for ---------------------------------------------------------------------

    for (int ii = m_st; ii <= nx1; ii += m_tw)
    {
        for (int jj = m_tw; jj <= nx2; jj += m_tw)
        {
            m_cse[ii][jj] -= m_css[ii][jj] * m_cse[ii     ][jj - m_st] +
                             m_cww[ii][jj] * m_cse[ii - m_st][jj     ] +
                             m_cee[ii][jj] * m_cse[ii + m_st][jj     ] +
                             m_cnn[ii][jj] * m_cse[ii     ][jj + m_st];
        }
    }
    // for ---------------------------------------------------------------------

    { // -----------------------------------------------------------------------
        if (!m_done2 && m_st <= 2)
        {
            m_done2 = true;
        }
    } // critical -------------------------------------------------------------

    if (!m_done2)
    {
        goto loop2;
    }

endloop2:; // while (m_st > 2)

    // note: the following is the same piece of code as the piece at the start of the preceding while loop

    { // -----------------------------------------------------------------------
        m_tw = m_st;
        m_st /= 2;
        m_niv -= 2;
        ASSERT(m_niv == 0,"not back where we started!");
    } // master ---------------------------------------------------------------

    for (int ii = m_st; ii <= nx1; ii += m_tw)
    {
        for (int jj = m_st; jj <= nx2; jj += m_tw)
        {
            m_cse[ii][jj] -= m_css[ii][jj] * m_cse[ii + m_st][jj - m_st] +
                             m_cww[ii][jj] * m_cse[ii - m_st][jj - m_st] +
                             m_cee[ii][jj] * m_cse[ii + m_st][jj + m_st] +
                             m_cnn[ii][jj] * m_cse[ii - m_st][jj + m_st];
        }
    }
    // for ---------------------------------------------------------------------

    // end of repeated code

     } // master temporary?
} // solver


/// solves system
/** In deze subroutine wordt S(x)=R opgelost, waarbij S het 
    Schurcomplement is dat ontstaat door alle 
    zwarte punten van een schaakbordvolgorde te elimineren. 
    We stoppen de iteratie zodra de 2-norm van het 
    residu tenminste een factor tol is afgenomen. 
    Het CG-algoritme is geimplementeerd zoals op blz. 
    146 in 'Solving Linear Systems on Vector and Shared 
    Memory Computers' van Dongarra, Duff, Sorensen en 
    v.d. Vorst is beschreven. 
    parameters:
    X: veld waarin de oplossing komt te staan. 
    B: veld met rechterlid van het stelsel vergelijkingen. 
       Deze vector wordt overschreven door het residu.  */
void RrbSolver::solve(Array2D<REAL> *pB, // real B(0:Nx1+2,0:Nx2+2)
                      Array2D<REAL> *pX) // real X(0:Nx1+2,0:Nx2+2)
{
    // this is done inside the WavesComputer::compute() parallelization
    {

    { //########################################################################

    // syntactic sugar
    Array2D<REAL> &B = *pB;
    Array2D<REAL> &X = *pX;

    Array2D<REAL> &cc = *m_pcc;
    Array2D<REAL> &cs = *m_pcs;
    Array2D<REAL> &cw = *m_pcw;

    const int nx1 = m_Nx1 + 1;
    const int nx2 = m_Nx2 + 1;
    const int nx12 = m_Nx1 + 2;
    const int nx22 = m_Nx2 + 2;

    { // -----------------------------------------------------------------------
        rhoold = 0.0;
        rhonew = 0.0;
        pq = 0.0;
        m_rhobew = 0.0;
        m_bet = 0.0;
        m_alf = 0.0;
        m_done = false;
    } // master ---------------------------------------------------------------

    // Alle zwarte punten worden exact geelimineerd: 
    // All black points are exactly eliminated:

    { // -----------------------------------------------------------------------
        B[0][0] -= cs[0][1] * B[0][1] * cc[0][1] + cw[1][0] * B[1][0] * cc[1][0];
    } // master nowait ---------------------------------------------------------

    for (int ii = 2; ii <= nx1; ii += 2)
    {
        B[ii][0] -= cs[ii    ][1] * B[ii    ][1] * cc[ii    ][1] +
                    cw[ii + 1][0] * B[ii + 1][0] * cc[ii + 1][0] +
                    cw[ii    ][0] * B[ii - 1][0] * cc[ii - 1][0];
    }
    // for nowait --------------------------------------------------------------

    for (int jj = 2; jj <= nx2; jj += 2)
    {
        B[0][jj] -= cs[0][jj + 1] * B[0][jj + 1] * cc[0][jj + 1] +
                    cw[1][jj    ] * B[1][jj    ] * cc[1][jj    ] +
                    cs[0][jj    ] * B[0][jj - 1] * cc[0][jj - 1];
    }
    // for nowait --------------------------------------------------------------

    for (int i1 = 1; i1 < nx1; i1 += 2)
    {
        int i2 = i1 + 1;
        for (int j1 = 1; j1 < nx2; j1 += 2)
        {
            int j2 = j1 + 1;
            B[i1][j1] -= cs[i1    ][j1 + 1] * B[i1    ][j1 + 1] * cc[i1    ][j1 + 1] +
                         cw[i1 + 1][j1    ] * B[i1 + 1][j1    ] * cc[i1 + 1][j1    ] +
                         cs[i1    ][j1    ] * B[i1    ][j1 - 1] * cc[i1    ][j1 - 1] +
                         cw[i1    ][j1    ] * B[i1 - 1][j1    ] * cc[i1 - 1][j1    ];
            B[i2][j2] -= cs[i2    ][j2 + 1] * B[i2    ][j2 + 1] * cc[i2    ][j2 + 1] +
                         cw[i2 + 1][j2    ] * B[i2 + 1][j2    ] * cc[i2 + 1][j2    ] +
                         cs[i2    ][j2    ] * B[i2    ][j2 - 1] * cc[i2    ][j2 - 1] +
                         cw[i2    ][j2    ] * B[i2 - 1][j2    ] * cc[i2 - 1][j2    ];
        }
    }
    // for ---------------------------------------------------------------------

    // We berekenen de vector S*X met S het 1e Schurcomplement. 
    matvec(X, &m_cse);

    for (int i1 = 1; i1 <= nx12; i1 += 2)
    {
        int i0 = i1 - 1;
        for (int j1 = 1; j1 <= nx22; j1 += 2)
        {
            int j0 = j1 - 1;
            B[i1][j1] -= m_cse[i1][j1];
            B[i0][j0] -= m_cse[i0][j0];
            m_cse[i1][j1] = B[i1][j1];
            m_cse[i0][j0] = B[i0][j0];
        }
    }
    // for ---------------------------------------------------------------------

    solver();

    { // -----------------------------------------------------------------------
        rhoold = 0.0;
    } // master ----------------------------------------------------------------

    for (int i1 = 1; i1 <= nx12; i1 += 2)
    {
        int i0 = i1 - 1;
        for (int j1 = 1; j1 <= nx22; j1 += 2)
        {
            int j0 = j1 - 1;
            rhoold += B[i0][j0] * m_cse[i0][j0] + B[i1][j1] * m_cse[i1][j1];
        }
    }
    // for ---------------------------------------------------------------------

    { // -----------------------------------------------------------------------
        rhonew = rhoold;
        m_rhobew = (rhonew + 1) * FloatUtils::square(m_tolerance);
        m_bet = 0.0;
        m_cne = 0.0;
        m_done = fabs(rhonew) <= fabs(m_rhobew);
    } // master ----------------------------------------------------------------

    // while (fabs(m_rhonew) > fabs(m_rhobew))
loop:
    if (m_done)
    {
        goto endloop;
    }

    for (int i1 = 1; i1 <= nx12; i1 += 2)
    {
        int i0 = i1 - 1;
        for (int j1 = 1; j1 <= nx22; j1 += 2)
        {
            int j0 = j1 - 1;
            m_cne[i1][j1] = m_cse[i1][j1] + m_bet * m_cne[i1][j1];
            m_cne[i0][j0] = m_cse[i0][j0] + m_bet * m_cne[i0][j0];
        }
    }
    // for ---------------------------------------------------------------------

    // We berekenen de vector S*P met S het 1e Schurcomplement en P de zoekrichting:
    matvec(m_cne, &m_cse);

    for (int ii = 0; ii <= nx1; ++ii)
    {
        for (int jj = 0; jj <= nx2; ++jj)
        {
            m_cnw[ii][jj] = m_cse[ii][jj];
        }
    }
    // for ---------------------------------------------------------------------

    { // -----------------------------------------------------------------------
        pq = 0.0;
    } // master ----------------------------------------------------------------

    for (int i1 = 1; i1 <= nx12; i1 += 2)
    {
        int i0 = i1 - 1;
        for (int j1 = 1; j1 <= nx22; j1 += 2)
        {
            int j0 = j1 - 1;
            pq += m_cne[i0][j0] * m_cnw[i0][j0] + m_cne[i1][j1] * m_cnw[i1][j1];
        }
    }
    // for ---------------------------------------------------------------------

    { // -----------------------------------------------------------------------
        m_alf = rhoold / pq;
    } // master ----------------------------------------------------------------

    for (int i1 = 1; i1 <= nx12; i1 += 2)
    {
        int i0 = i1 - 1;
        for (int j1 = 1; j1 <= nx22; j1 += 2)
        {
            int j0 = j1 - 1;
            X[i1][j1] += m_alf * m_cne[i1][j1];
            X[i0][j0] += m_alf * m_cne[i0][j0];
            B[i1][j1] -= m_alf * m_cnw[i1][j1];
            B[i0][j0] -= m_alf * m_cnw[i0][j0];
            m_cse[i1][j1] = B[i1][j1];
            m_cse[i0][j0] = B[i0][j0];
        }
    }
    // for ---------------------------------------------------------------------

    solver();

    { // -----------------------------------------------------------------------
        rhonew = 0.0;
    } // master ----------------------------------------------------------------

    for (int i1 = 1; i1 <= nx12; i1 += 2)
    {
        int i0 = i1 - 1;
        for (int j1 = 1; j1 <= nx22; j1 += 2)
        {
            int j0 = j1 - 1;
            rhonew += B[i0][j0] * m_cse[i0][j0] + B[i1][j1] * m_cse[i1][j1];
        }
    }
    // for ---------------------------------------------------------------------

    { // -----------------------------------------------------------------------
        m_bet = rhonew / rhoold;
        rhoold = rhonew;
    } // master ----------------------------------------------------------------

    { // -----------------------------------------------------------------------
        if (!m_done && fabs(rhonew) <= fabs(m_rhobew))
        {
            m_done = true;
        }
    } // critical --------------------------------------------------------------

    if (!m_done)
    {
        goto loop;
    }
endloop:;

    // Alle componenten van de eerste groep berekenen door 
    // terugsubstitutie: X1=D^(-1)*(RL1-A12(X2)) 
    for (int ii = 1; ii <= nx1; ii += 2)
    {
        X[ii][0] = (B[ii][0] -
                    cs[ii    ][1] * X[ii    ][1] -
                    cw[ii + 1][0] * X[ii + 1][0] -
                    cw[ii    ][0] * X[ii - 1][0]) * cc[ii][0];
    }
    // for nowait --------------------------------------------------------------

    for (int jj = 1; jj <= nx2; jj += 2)
    {
        X[0][jj] = (B[0][jj] -
                    cs[0][jj + 1] * X[0][jj + 1] -
                    cw[1][jj    ] * X[1][jj    ] -
                    cs[0][jj    ] * X[0][jj - 1]) * cc[0][jj];
    }
    // for nowait --------------------------------------------------------------

    for (int i1 = 1; i1 < nx1; i1 += 2)
    {
        int i2 = i1 + 1;
        for (int j1 = 1; j1 < nx2; j1 += 2)
        {
            int j2 = j1 + 1;
            X[i1][j2] = (B[i1][j2] -
                         cs[i1    ][j2 + 1] * X[i1    ][j2 + 1] -
                         cw[i1 + 1][j2    ] * X[i1 + 1][j2    ] -
                         cw[i1    ][j2    ] * X[i1 - 1][j2    ] -
                         cs[i1    ][j2    ] * X[i1    ][j2 - 1]) * cc[i1][j2];
            X[i2][j1] = (B[i2][j1] -
                         cs[i2    ][j1 + 1] * X[i2    ][j1 + 1] -
                         cw[i2 + 1][j1    ] * X[i2 + 1][j1    ] -
                         cw[i2    ][j1    ] * X[i2 - 1][j1    ] -
                         cs[i2    ][j1    ] * X[i2    ][j1 - 1]) * cc[i2][j1];
        }
    }
    // for ---------------------------------------------------------------------

    } // #######################################################################

    } // master openmp1
} // solve 


/// matrix vector product
/** Deze subroutine berekent de vector Az=S*X met S het Schurcomplement dat 
    ontstaat door alle zwarte punten van een schaakbordvolgorde te elimineren.  */
void RrbSolver::matvec(const Array2D<REAL> &X, //
                       Array2D<REAL> *pAz)
{
    // syntactic sugar
    Array2D<REAL> &Az = *pAz;
    const Array2D<REAL> &cc = *m_pcc;
    const Array2D<REAL> &cs = *m_pcs;
    const Array2D<REAL> &cw = *m_pcw;

    const int nx1 = m_Nx1 + 1;
    const int nx2 = m_Nx2 + 1;

    // Note: Az is used both as scratch and as result array.
    // This is possible because of the checkerboard algorithm.
    // The scratch checkerboard color starts at Az[1][0].
    // The result checkerboard color starts at Az[0][0].
    // The scratch Az elements must be calculated completely
    // before the result Az elements are calculated!
    
    for (int ii = 1; ii <= nx1; ii += 2)
    {
        Az[ii][0] = (cs[ii    ][1] * X[ii    ][1] +
                     cw[ii + 1][0] * X[ii + 1][0] +
                     cw[ii    ][0] * X[ii - 1][0]) * cc[ii][0];
    }
    // for----------------------------------------------------------------------

    for (int jj = 1; jj <= nx2; jj += 2)
    {
        Az[0][jj] = (cs[0][jj + 1] * X[0][jj + 1] +
                     cw[1][jj    ] * X[1][jj    ] +
                     cs[0][jj    ] * X[0][jj - 1]) * cc[0][jj];
    }
    // for----------------------------------------------------------------------

    for (int i1 = 1; i1 < nx1; i1 += 2)
    {
        int i2 = i1 + 1;
        for (int j2 = 2; j2 < nx2; j2 += 2)
        {
            int j1 = j2 - 1;
            Az[i1][j2] = (cs[i1    ][j2 + 1] * X[i1    ][j2 + 1] +
                          cw[i1 + 1][j2    ] * X[i1 + 1][j2    ] +
                          cs[i1    ][j2    ] * X[i1    ][j2 - 1] +
                          cw[i1    ][j2    ] * X[i1 - 1][j2    ]) * cc[i1][j2];
            Az[i2][j1] = (cs[i2    ][j1 + 1] * X[i2    ][j1 + 1] +
                          cw[i2 + 1][j1    ] * X[i2 + 1][j1    ] +
                          cs[i2    ][j1    ] * X[i2    ][j1 - 1] +
                          cw[i2    ][j1    ] * X[i2 - 1][j1    ]) * cc[i2][j1];
        }
    }
    // for----------------------------------------------------------------------

    { //------------------------------------------------------------------------
    Az[0][0] = cc[0][0] * X [0][0] -
               cs[0][1] * Az[0][1] -
               cw[1][0] * Az[1][0];
    } // master ------------------------------------------------------------------

    for (int ii = 2; ii <= nx1; ii += 2)
    {
        Az[ii][0] = cc[ii    ][0] * X [ii    ][0] -
                    cs[ii    ][1] * Az[ii    ][1] -
                    cw[ii + 1][0] * Az[ii + 1][0] -
                    cw[ii    ][0] * Az[ii - 1][0];
    }
    // for----------------------------------------------------------------------

    for (int jj = 2; jj <= nx2; jj += 2)
    {
        Az[0][jj] = cc[0][jj    ] * X [0][jj    ] -
                    cs[0][jj + 1] * Az[0][jj + 1] -
                    cw[1][jj    ] * Az[1][jj    ] -
                    cs[0][jj    ] * Az[0][jj - 1];
    }
    // for----------------------------------------------------------------------

    for (int i1 = 1; i1 < nx1; i1 += 2)
    {
        int i2 = i1 + 1;
        for (int j1 = 1; j1 < nx2; j1 += 2)
        {
            int j2 = j1 + 1;
            Az[i1][j1] = cc[i1    ][j1    ] * X [i1    ][j1    ] -
                         cs[i1    ][j1 + 1] * Az[i1    ][j1 + 1] -
                         cw[i1 + 1][j1    ] * Az[i1 + 1][j1    ] -
                         cs[i1    ][j1    ] * Az[i1    ][j1 - 1] -
                         cw[i1    ][j1    ] * Az[i1 - 1][j1    ];
            Az[i2][j2] = cc[i2    ][j2    ] * X [i2    ][j2    ] -
                         cs[i2    ][j2 + 1] * Az[i2    ][j2 + 1] -
                         cw[i2 + 1][j2    ] * Az[i2 + 1][j2    ] -
                         cs[i2    ][j2    ] * Az[i2    ][j2 - 1] -
                         cw[i2    ][j2    ] * Az[i2 - 1][j2    ];
        }
    }
    // for----------------------------------------------------------------------

} // matvec
